#include "os_include.h"

uint32_t             g_PrioGrop          = 0;  /*任务优先级组*/
uint8_t              g_PrioTable[32]     = {0};/*任务优先级表*/
static const uint8_t g_PrioUnpckMap[256] =     /*解码表*/
{
0x0, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x00-0x0F*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x10-0x1F*/
0x5, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x20-0x2F*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x30-0x3F*/
0x6, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x40-0x4F*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x50-0x5F*/
0x5, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x60-0x6F*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x70-0x7F*/
0x7, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x80-0x8F*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0x90-0x9F*/
0x5, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0xA0-0xAF*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0xB0-0xBF*/
0x6, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0xC0-0xCF*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0xD0-0xDF*/
0x5, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0xE0-0xEF*/
0x4, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, 0x3, 0x0, 0x1, 0x0, 0x2, 0x0, 0x1, 0x0, /*0xF0-0xFF*/   
};


uint32_t g_OSTickCount     = 0; /*系统心跳计数器*/
uint32_t g_OSIdleRunCount  = 0; /*系统空闲任务计数器*/
uint32_t g_OSTimerRunCount = 0; /*系统定时器*/
uint32_t g_OSRuningFlag    = 0; /*系统运行标志 =1系统开始运行*/
uint32_t g_OSSuspendSchedulerCount = 0;


T_LIST g_OSTaskReadyTable[config_SYS_MAX_TASK_NUM]; /*存放就绪态的任务*/
T_LIST g_OSTaskDelayList[2];                        /*用于存放taskDelay挂起的任务*/
T_LIST g_OSTaskPendList;                            /*用于存放挂起态的任务*/
T_LIST *g_OSTaskDelayListNowPt      = NULL;         /*延时列表指针（当前正在使用）*/
T_LIST *g_OSTaskDelayListOverflowPt = NULL;         /*延时列表指针（超时列表）*/
T_LIST *g_OSTaskReadyListPt         = NULL;         /*就绪态任务列表指针*/
T_LIST *g_OSTaskPendListPt          = NULL;         /*挂起态任务列表指针*/  
OS_WORD g_OSTimeOutTimeNow          = OS_MAX_DELAY; /*距离最近的延时任务到达时间（当前）*/
OS_WORD g_OSTimeOutTimeOverflow     = OS_MAX_DELAY; /*距离最近的延时任务到达时间（超时）*/
OS_TCB *g_OSTcbCurrentPt;                           /*当前正在运行的任务TCB指针 */
OS_TCB *g_OSTcbNextPt;                              /*下一个要运行的任务TCB*/

/*没有使用此堆栈，MSP堆栈大小由startUp文件指定*/
OS_STK g_OSExceptStack[CPU_EXCEPTSTACK_SIZE]; /*MSP堆栈大小*/
OS_STK *g_OSExceptStackBasePt = &g_OSExceptStack[CPU_EXCEPTSTACK_SIZE-1]; /*MSP堆栈指针*/

OS_TCB g_OSIdleTaskTCB;
OS_STK g_OSIdleTaskStk[config_IDLE_TASK_STK_SIZE];

OS_TCB g_OSTimerTaskTCB;
OS_STK g_OSTimerTaskStk[config_TIMER_TASK_STK_SIZE];

void OSTimerTask(void* aParam)
{
    aParam = aParam;
    g_OSRuningFlag = 1;
    while(1)
    {
        ++g_OSIdleRunCount;
        OSTaskDelay(1);
    }
}

void OSIdleTask(void* aParam)
{
    aParam = aParam;
    while(1)
    {
        ++g_OSIdleRunCount;
    }
}

void OSIdelTaskInit(void)
{
    OSTaskCreate(&g_OSIdleTaskTCB, OSIdleTask, NULL, config_SYS_MAX_TASK_NUM-1, g_OSIdleTaskStk, config_IDLE_TASK_STK_SIZE);  
}

void OSTimerTaskInit(void)
{
    OSTaskCreate(&g_OSTimerTaskTCB, OSTimerTask, NULL, 1, g_OSTimerTaskStk, config_TIMER_TASK_STK_SIZE);  
}

void OSTaskEnd(void* param)
{
    while(1)
    {
    
    }
}

OS_TICK OSTickGet(void)
{
    return g_OSTickCount;
}

OS_TICK OSTaskTickGet(void)
{
    return g_OSTcbCurrentPt->usedTime;
}

void OSStakInit(OS_TCB *aTcb,OS_taskFun aTask, void *aParam, OS_STK *aStack, uint32_t aStkSize)
{
    OS_STK *pstrStack;
    pstrStack = aStack+aStkSize;
    pstrStack = (OS_STK*) ((OS_STK)(pstrStack)&0xfffffff8u);/* 8字节对齐 */
    *(--pstrStack) = (unsigned int)0x01000000ul;    /* XPSR*/
    *(--pstrStack) = (unsigned int)aTask;            /* r15 */
    *(--pstrStack) = (unsigned int)OSTaskEnd;        /* r14 */
    *(--pstrStack) = (unsigned int)0x12121212ul;    /* r12 */
    *(--pstrStack) = (unsigned int)0x03030303ul;    /* r3  */
    *(--pstrStack) = (unsigned int)0x02020202ul;    /* r2  */
    *(--pstrStack) = (unsigned int)0x01010101ul;    /* r1  */
    *(--pstrStack) = (unsigned int)aParam;    /* r0  */

    *(--pstrStack) = (unsigned int)0x11111111ul;    /* r11 */
    *(--pstrStack) = (unsigned int)0x10101010ul;    /* r10 */
    *(--pstrStack) = (unsigned int)0x09090909ul;    /* r9  */
    *(--pstrStack) = (unsigned int)0x08080808ul;    /* r8  */
    *(--pstrStack) = (unsigned int)0x07070707ul;    /* r7  */
    *(--pstrStack) = (unsigned int)0x06060606ul;    /* r6  */
    *(--pstrStack) = (unsigned int)0x05050505ul;    /* r5  */
    *(--pstrStack) = (unsigned int)0x04040404ul;    /* r4  */
    aTcb->OSTCBStkPtr = pstrStack;
}

uint8_t OSGetPrio(void)
{
    uint8_t x,y;
    if(g_PrioGrop & 0x000000FF)
    {
        x = g_PrioUnpckMap[g_PrioGrop&0x000000FF];
        y = g_PrioUnpckMap[g_PrioTable[x]];
    }
    else if(g_PrioGrop & 0x0000FF00)
    {
        x = g_PrioUnpckMap[(g_PrioGrop>>8)&0x000000FF]+8;
        y = g_PrioUnpckMap[g_PrioTable[x]];
    }
    else if((g_PrioGrop & 0x00FF0000))
    {
        x = g_PrioUnpckMap[(g_PrioGrop>>16)&0x000000FF]+16;
        y = g_PrioUnpckMap[g_PrioTable[x]];
    }
    else
    {
        x = g_PrioUnpckMap[(g_PrioGrop>>24)&0x000000FF]+24;
        y = g_PrioUnpckMap[g_PrioTable[x]];
    }
    return ((x<<3) + y);
}

void OSSetPrio(uint8_t aPrio)
{
    uint8_t x;
    if(aPrio >= config_SYS_MAX_TASK_NUM)
    {
        aPrio = config_SYS_MAX_TASK_NUM-1;
    }
    x = aPrio>>3;
    g_PrioGrop     |= 1<<x;
    g_PrioTable[x] |= 1<<(aPrio&0x07);    
}

void OSClearPrio(uint8_t aPrio)
{
    if(g_OSTaskReadyTable[aPrio].length == 0)
    {
        uint8_t x = aPrio>>3;
        g_PrioTable[x] &= ~(1<<(aPrio&0x07));
        if(g_PrioTable[x] == 0)
        {
            g_PrioGrop &= ~(1<<x);
        }
    }
}

void OSGetHighestTask(void)
{
    OS_WORD highestPrio;
    
    OS_ENTER_CRITICAL(); /*临界段保护 打开*/
    if(g_OSSuspendSchedulerCount)
    {
        return;
    }
    highestPrio   = OSGetPrio();
    g_OSTcbNextPt = (OS_TCB*)g_OSTaskReadyTable[highestPrio].slider;

    if(g_OSTaskReadyTable[highestPrio].length)
    {
        if( (--g_OSTcbNextPt->leftTime)==0 ) /*当前任务时间片已经用完*/
        {
            g_OSTcbNextPt->leftTime                = config_SYS_TIME_SLICE_SIZE; /*恢复时间片计数值*/
            g_OSTcbNextPt                          = g_OSTcbNextPt->next;        /*获取下一个有时间片的相同优先级的任务控制块*/
            g_OSTaskReadyTable[highestPrio].slider = g_OSTaskReadyTable[highestPrio].slider->next;
        }
    }
    ++g_OSTcbNextPt->usedTime; /*任务占用时间增加*/
    OS_EXIT_CRITICAL(); /*临界段保护 关闭*/
    
    if(g_OSTcbNextPt != g_OSTcbCurrentPt)
    {
        OSCtxSw();
    }
}

void OSGetTimeOutTask(void)
{
    if(g_OSTimeOutTimeNow == g_OSTickCount)
    {
        T_LIST *listPt = NULL;
        OS_TCB *tcbP   = NULL;
        if(g_OSTaskDelayListNowPt->length)
        { 
            while(1)
            {
                tcbP   = (OS_TCB*)ListDel(g_OSTaskDelayListNowPt, 0);
                listPt = &g_OSTaskReadyTable[tcbP->prio];
                OSSetPrio(tcbP->prio);
                ListInsert(listPt, (T_Node*)tcbP, 0);
                if(g_OSTaskDelayListNowPt->length)
                {
                    tcbP = (OS_TCB*)g_OSTaskDelayListNowPt->next;
                    if(tcbP->nextTime != g_OSTimeOutTimeNow)
                    {
                        g_OSTimeOutTimeNow = tcbP->nextTime;
                        break;
                    }
                }
                else
                {
                    g_OSTimeOutTimeNow = OS_MAX_DELAY;
                    break;
                }
            }
        }
    }
}

void OSSwTask(void)
{
    OSGetHighestTask();
}

extern uint8_t taskTest;

void OSTick(void)
{
    if(g_OSRuningFlag)
    {
        taskTest = 1;
        if( (++g_OSTickCount)==0 ) /*系统节拍溢出*/
        {
            T_LIST *tempP;
            tempP = g_OSTaskDelayListNowPt;
            g_OSTaskDelayListNowPt = g_OSTaskDelayListOverflowPt;
            g_OSTaskDelayListOverflowPt = tempP;
            
            g_OSTimeOutTimeNow = g_OSTimeOutTimeOverflow;
            g_OSTimeOutTimeOverflow = OS_MAX_DELAY;
        }
        OSGetTimeOutTask();
        OSSwTask();
        taskTest = 0;
    }
}

OS_TASK_ID OSTaskCreate(OS_TCB* aTcb, OS_taskFun aTask, void *aParam, uint16_t aPrio, OS_STK* aStack, uint32_t aStkSize)
{
    T_LIST *listPt = NULL;
    OS_ENTER_CRITICAL(); /*临界段保护 打开*/
    
    if(aPrio>=config_SYS_MAX_TASK_NUM)
    {
        aPrio = config_SYS_MAX_TASK_NUM-1;
    }
    aTcb->prio = aPrio;
    aTcb->leftTime = config_SYS_TIME_SLICE_SIZE;
    
    listPt = &g_OSTaskReadyTable[aPrio];
    
    OSSetPrio(aPrio);
    
    ListInsert(listPt, (T_Node*)aTcb, 0);
    
    OSStakInit(aTcb, aTask, aParam, aStack, aStkSize);
    OS_EXIT_CRITICAL(); /*临界段保护 关闭*/
    return ((OS_TASK_ID)aTcb);
}

void OSTaskDelete(OS_TASK_ID aTaskHandle)
{
    OS_TCB* tcbP = (OS_TCB*)aTaskHandle;   
    OS_ENTER_CRITICAL(); /*临界段保护 打开*/
    if(tcbP == NULL)
    {
        tcbP = g_OSTcbCurrentPt;
    }
    OSClearPrio(tcbP->prio);
    ListDelNode((T_Node*)tcbP);
    OS_EXIT_CRITICAL(); /*临界段保护 关闭*/
    OSSwTask();
}

void OSTaskDelay(OS_WORD aTick)
{
    OS_ENTER_CRITICAL(); /*临界段保护 打开*/
    ListDelNode((T_Node*)g_OSTcbCurrentPt);
    OSClearPrio(g_OSTcbCurrentPt->prio);
    g_OSTcbCurrentPt->nextTime = g_OSTickCount+aTick; /*计算唤醒时间*/
    if(g_OSTcbCurrentPt->nextTime < g_OSTickCount) /*时间溢出*/
    {
        ListInsertMax(g_OSTaskDelayListOverflowPt, (T_Node*)g_OSTcbCurrentPt);
        if(g_OSTimeOutTimeOverflow > g_OSTcbCurrentPt->nextTime)
        {
            g_OSTimeOutTimeOverflow = g_OSTcbCurrentPt->nextTime;
        }
    }
    else
    {
        ListInsertMax(g_OSTaskDelayListNowPt, (T_Node*)g_OSTcbCurrentPt);
        if(g_OSTimeOutTimeNow > g_OSTcbCurrentPt->nextTime)
        {
            g_OSTimeOutTimeNow = g_OSTcbCurrentPt->nextTime;
        }
    }
    OS_EXIT_CRITICAL(); /*临界段保护 关闭*/
    OSSwTask();
}

void OSSchedulerSuspend(void)
{
    ++g_OSSuspendSchedulerCount;
}

void OSSchedulerResume(void)
{
    OS_ENTER_CRITICAL(); /*临界段保护 打开*/
    if(g_OSSuspendSchedulerCount>0)
    {
        --g_OSSuspendSchedulerCount;
    }
    OS_EXIT_CRITICAL(); /*临界段保护 关闭*/
}

void OSInit(void)
{
    OS_WORD i;
    T_LIST *listPt = NULL;
    OS_ENTER_CRITICAL();
    
    /*任务列表初指针初始化*/
    g_OSTaskDelayListNowPt       = &g_OSTaskDelayList[0];
    g_OSTaskDelayListOverflowPt  = &g_OSTaskDelayList[1];
    g_OSTaskReadyListPt          = &g_OSTaskReadyTable[0];
    g_OSTaskPendListPt           = &g_OSTaskPendList;
    
    /*初始化任务列表*/
    ListInit(g_OSTaskDelayListNowPt);
    ListInit(g_OSTaskDelayListOverflowPt);
    ListInit(g_OSTaskPendListPt);

    for(i=0; i<config_SYS_MAX_TASK_NUM; i++)
    {
        listPt = &g_OSTaskReadyTable[i];
        ListInit(listPt);
    }
    
    OSIdelTaskInit();
    OSTimerTaskInit();
    
    g_OSTcbCurrentPt = &g_OSIdleTaskTCB;
    g_OSTcbNextPt    = &g_OSIdleTaskTCB;
    
    OS_EXIT_CRITICAL();
}
